#ifndef _DETECTOR_H_
#define _DETECTOR_H_

#include "pins.h"
#include <Servo.h>

#define SAMPLING_COUNT      6
#define MIN_DISTANCE       15                     // 最小距离，厘米

class Detector {
private:
    Servo servo;
    double distance;
    const int DEFAULT_ANGLE = 99;
    const int LEFT          = DEFAULT_ANGLE + 30;
    const int RIGHT         = DEFAULT_ANGLE - 30;

    double temp_value [SAMPLING_COUNT], values [3];

    double getDistance (int angle);
public :
    Detector ();
    double getDistance ();
    int    scan ();
    int    scan1 ();
    void   scan2 (double *holder);
    void   reset ();
};

Detector::Detector () {
    servo.attach (PIN_DETECTOR);
    pinMode (PIN_ECHO, INPUT);
    pinMode (PIN_TRIG, OUTPUT);
    digitalWrite (PIN_TRIG, LOW);
    servo.write (DEFAULT_ANGLE);

    for (int i = 0; i < SAMPLING_COUNT; i ++) {
        temp_value [i] = 0;
    }

    for (int i = 0; i < 3; i ++) {
        values [i] = 0.0;
    }
}

double Detector::getDistance () {
    int max = 0, min = 0, value;
    double dd;
    for (int i = 0; i < SAMPLING_COUNT; i ++) {
        digitalWrite (PIN_TRIG, LOW);
        delayMicroseconds (2);
        digitalWrite (PIN_TRIG, HIGH);
        delayMicroseconds (10);
        digitalWrite (PIN_TRIG, LOW);
    
        value = pulseIn (PIN_ECHO, HIGH);
        dd = value / 58.00;
        temp_value [i] = dd;

        if (dd > temp_value [max]) {
            max = i;
        }

        if (dd < temp_value [min]) {
            min = i;
        }
        delay (60);
    }

    double sum = 0;
    for (int i = 0; i < SAMPLING_COUNT; i ++) {
        if (i != max && i != min) {
            sum += temp_value [i];
        }
    }

    return sum / (SAMPLING_COUNT - (max == min ? 1 : 2));
}

double Detector::getDistance (int angle) {
    float theata = abs (((float)(DEFAULT_ANGLE - angle)) * PI / 180.0);
    servo.write (angle);
    delay (100);
    double d = getDistance ();
    Serial.print ("angle = ");
    Serial.print (angle);
    Serial.print (", theata = ");
    Serial.print (theata);
    Serial.print (", d = ");
    Serial.println (d);
    return theata <= 0.001 ? d : d * sin (theata);
}

void Detector::reset () {
    servo.write (DEFAULT_ANGLE);
}

void Detector::scan2 (double *holder) {
    *(holder ++) = getDistance (LEFT);
    *(holder ++) = getDistance (DEFAULT_ANGLE);
    *(holder ++) = getDistance (RIGHT);
}

int Detector::scan1 () {
    double left, front, right;
    left  = getDistance (LEFT);
    front = getDistance (DEFAULT_ANGLE);
    right = getDistance (RIGHT);

#ifdef DEBUG
    Serial.print ("left = ");
    Serial.print (left);
    Serial.print (", front = ");
    Serial.print (front);
    Serial.print (", right = ");
    Serial.println (right);
#endif

    double m = min (left, min (front, right));
    if (m > MIN_DISTANCE) return 3;
    if (m == left)        return m < MIN_DISTANCE ? -1 : 2;
    if (m == front)       return 0;
    if (m == right)       return m < MIN_DISTANCE ? -2 : 1;
    return 5;
}

int Detector::scan () {
    int i = 0, 
        j = 0,                              // 最小距离角度序号
        k = 0;                              // 最大距离角度序号
    double max_value = 0, min_value, value;
    // 获取前方7个角度的距离，分别为45, 60, 75, 90, 105, 120, 135
    // 计算出最小距离的角度，和最大距离的角度
    for (int a = LEFT; a >= RIGHT; a -= 15) {
        servo.write (a);                    // 舵机转到指定角度
        delay (100);                        // 等待舵机转动结束
        value = getDistance ();             // 获取当前角度的距离
        if (i == 0) {
            min_value = max_value = value;  
        } else {
            if (min_value > value) {
                min_value = value;
                j = i;
            }

            if (max_value < value) {
                max_value = value;
                k = i;
            }
        }
        i ++;
    }
    servo.write (DEFAULT_ANGLE);            // 复位舵机角度

    if (max_value < MIN_DISTANCE) {
        // 若最大距离小于指定的距离，则直接倒车
        // 若最大距离的角度出现在3之前，意味着车头右侧离障碍物更近，
        // 应该在倒车的同时右转
        if (k < 3) {
            return -2;                      // 右转倒车
        } else {
            return -1;                      // 左转倒车
        }
    } else {                                // 至少一个方向上的距离足够
        // 若最小距离的角度出现在3之前，以为着车头左侧离障碍物更近一些，
        // 这时应该右转
        if (j < 3) {
            return 2;                       // 右转前进
        } else if (j == 3) {
            // 障碍物在车头的正前方，倒车
            return 0;                       // 直线倒车
        } else {                            
            return 1;                       // 左转前进
        }
    }
}

#endif
